import math
from math import radians, sin, cos, atan2, degrees

# import rospy

"""
计算无人机飞行点相关功能工具类
"""

earth_radius = 6371000


class DroneUtil:
    # 地球平均半径（米）

    def __init__(self):
        pass

    @staticmethod
    def cut_fly_points(fly_points, num=20):
        """
        裁减飞行点
        fly_points 塔机自驾点
        num 保留点的个数
        """

        if len(fly_points) <= num:
            return fly_points
        first = fly_points[0]
        last = fly_points[-1]

        # 数组中间的点
        mid = fly_points[1: -1]
        remove_num = num - 2
        step = len(mid) // remove_num
        select_mid = mid[::step][:remove_num]
        return [first] + select_mid + [last]

    @staticmethod
    # 点位计算
    def calculate_rotated_point(origin_lat, origin_lon, radius_meters, n, interval_degrees=90):
        """
        计算以给定原点为中心，半径为radius_meters的圆上，每隔interval_degrees度的第n个点的经纬度。

        :param origin_lat: 原点纬度（度）
        :param origin_lon: 原点经度（度）
        :param radius_meters: 半径（米）
        :param n: 旋转后的第几个点
        :param interval_degrees: 每隔多少度计算一个点（默认90度）
        :return: 旋转后的点的经纬度（纬度, 经度）
        """
        # rospy.loginfo(
        #     f"入参:原点纬度:{origin_lat}, 原点经度:{origin_lon}, 半径:{radius_meters}, 旋转后的第{n}个点, 每隔{interval_degrees}度计算一个点")

        # 计算纬度变化
        delta_latitude = radius_meters / earth_radius * (180 / math.pi)

        # 计算经度变化
        delta_longitude = radius_meters / (earth_radius * math.cos(origin_lat * math.pi / 180)) * (180 / math.pi)

        # 计算旋转角度
        angle = n * interval_degrees

        # 计算旋转后的点的经纬度
        new_lat = origin_lat + delta_latitude * math.sin(math.radians(angle))
        new_lon = origin_lon + delta_longitude * math.cos(math.radians(angle))

        # 保留8位小数
        new_lat = round(new_lat, 10)
        new_lon = round(new_lon, 10)
        # 日志
        # rospy.loginfo(f"旋转后的点的经纬度: 纬度 {new_lat}° N, 经度 {new_lon}° E")
        return new_lat, new_lon


if __name__ == '__main__':
    origin_lat = 22.309534904776  # 原始纬度
    origin_lon = 113.458745023791  # 原始经度
